#ifndef SERIAL_COMM_
#define SERIAL_COMM_

#include "include/serial_comm.h"

SerialComm::SerialComm(const char* port, int baud_in){
  struct termios toptions;
  
  //Set field
  valid = false;

#ifdef VERBOSE
  fprintf(stderr, "Attempting to open serial connection on port: %s @ baud %d\n",
	  port, baud);
#endif

  // As per new POSIX convetion the usage of NONBLOCK is over NDELAY
  // O_RDW is read and write
  // NOCTTY makes sure we don't own the process on the termnal device

  fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
  if (fd < 0){
    fprintf(stderr, "Unable to open serial connection.\n");
    return;
  }

  // Name says all
  if (tcgetattr(fd, &toptions)) {
    fprintf(stderr, "Unable to get fd attributes.\n");
    return;
  }

  // Find the correct baud
  switch(baud_in) {
  case 4800: brate = B4800; break;
  case 9600: brate = B9600; break;
  case 19200: brate = B19200; break;
  case 38400: brate = B38400; break;
  case 57600: brate = B57600; break;
  case 115200: brate = B115200; break;
  default: 
    fprintf(stderr, "Unable to switch on baud rate.\n");
    return;
  }

  // Set the baud rate of the input and output
  cfsetispeed(&toptions, brate);
  cfsetospeed(&toptions, brate);

  // Recall 8N1 is need for arduino, this is no parity
  // c_cflag is control modes
  toptions.c_cflag &= ~PARENB;
  toptions.c_cflag &= ~CSTOPB;

  // Sets the character size bits
  toptions.c_cflag &= ~CSIZE;
  toptions.c_cflag |= CS8;

  // Disable hardware flow control
  toptions.c_cflag &= ~CRTSCTS;

  // Enable read and ignore modem control lines
  toptions.c_cflag |= CREAD | CLOCAL;
  
  // Turn off X flow control
  toptions.c_iflag &= ~(IXON | IXOFF | IXANY);

  // Create raw, c_lflag is local modes
  toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

  // Create raw output, c_oflag is output mode
  toptions.c_oflag &= ~OPOST;

  // VMIN is a character count 0-255
  // VTIME is time measured in seconds
  // If VMIN is zero then this is purely timed read.
  toptions.c_cc[VMIN] = 0;
  toptions.c_cc[VTIME] = 20;

  if(tcsetattr(fd, TCSANOW, &toptions) < 0){
    fprintf(stderr, "Unable to set term attributes.\n");
    return;
  }
  
  portName = port;
  valid = true;
}

int SerialComm::sendString(unsigned char* input, int length){
  int size = length * sizeof(unsigned char);
  int bytes_sent = 0;
  while(bytes_sent < size){
    int increment = write(fd, input+bytes_sent, size-bytes_sent);
    if(increment == -1){
      perror("increment of -1");
      //      exit(1);
      sleep(1);
      continue;
    }
    bytes_sent += increment;
  }
  
  return SUCCESS_COMM;
}

int SerialComm::readString(unsigned char** buff, char end){
  std::vector<unsigned char> vec;
  char b[1];
  int i = 0;
  do {
    int n = read(fd, b, 1);
    
    if(n == -1){
      fprintf(stderr, "In Serial::readString, recieved a neg one.\n");
      perror("Error:");
      return FAILURE_COMM;
    }

    if(n == 0){
      usleep(10*1000);
      continue;
    }
    vec.push_back(b[0]);
    i++;
  } while (b[0] != end);

  int size = vec.size();
  unsigned char* output = new unsigned char[size];
  for(int i = 0; i < size; ++i){
    output[i] = vec[i];
  }
  *buff = output;
  
  return SUCCESS_COMM;
}

bool SerialComm::isValid(void){
  return valid;
}
#endif
